function controls = generate_smart_trajectory(dt, total_time, obstacles, initial_state, map_size)
% 基于障碍物地图的智能轨迹生成器
% 该函数会分析障碍物布局，规划一条避开所有障碍物和地图边界的安全探索路径
%
% 输入:
%   dt - 时间步长
%   total_time - 总时间
%   obstacles - 障碍物列表 [x_min, y_min, x_max, y_max]
%   initial_state - 初始位姿 [x; y; theta]
%   map_size - 地图尺寸 [width, height] (可选，默认[50, 50])
%
% 输出:
%   controls - 控制序列 [v; omega] 每列一个时间步

    if nargin < 5
        map_size = [50, 50];
    end
    
    steps = total_time / dt;
    controls = zeros(2, steps);
    
    % 显示障碍物信息
    fprintf('\n=== 智能轨迹规划 ===\n');
    fprintf('地图尺寸: %.0fx%.0f\n', map_size(1), map_size(2));
    fprintf('障碍物数量: %d\n', size(obstacles, 1));
    fprintf('初始位置: (%.1f, %.1f, %.2f)\n', initial_state(1), initial_state(2), initial_state(3));
    
    % 分析障碍物分布，识别安全通道
    safety_margin = 1.5;  % 安全边距
    boundary_margin = 3.0; % 边界安全边距
    
    % 定义安全边界
    safe_bounds = struct();
    safe_bounds.x_min = boundary_margin;
    safe_bounds.x_max = map_size(1) - boundary_margin;
    safe_bounds.y_min = boundary_margin;
    safe_bounds.y_max = map_size(2) - boundary_margin;
    
    % 生成路径点（waypoints）
    waypoints = generate_safe_waypoints(obstacles, initial_state, safe_bounds, safety_margin);
    
    fprintf('规划的路径点数量: %d\n', size(waypoints, 2));
    fprintf('预计路径长度: %.1f m\n', calculate_path_length(waypoints));
    
    % 将路径点转换为控制序列
    controls = waypoints_to_controls(waypoints, dt, total_time, initial_state);
    
    fprintf('轨迹生成完成!\n\n');
end

function waypoints = generate_safe_waypoints(obstacles, initial_state, safe_bounds, safety_margin)
% 生成避开障碍物的安全路径点
    
    % 提取障碍物信息
    inner_obstacles = obstacles(5:end, :);  % 跳过外墙
    
    % 定义探索网格点（在安全区域内）
    x_points = linspace(safe_bounds.x_min + 2, safe_bounds.x_max - 2, 6);
    y_points = linspace(safe_bounds.y_min + 2, safe_bounds.y_max - 2, 6);
    
    % 筛选出安全的探索点
    safe_points = [];
    for i = 1:length(x_points)
        for j = 1:length(y_points)
            point = [x_points(i); y_points(j)];
            if is_point_safe(point, obstacles, safety_margin + 1.0)
                safe_points = [safe_points, point];
            end
        end
    end
    
    if isempty(safe_points)
        % 如果没有安全点，使用保守的中心路径
        fprintf('警告: 未找到足够的安全探索点，使用保守路径\n');
        waypoints = generate_conservative_path(initial_state, safe_bounds);
        return;
    end
    
    % 从初始位置开始，贪婪地选择最近的未访问安全点
    current_pos = initial_state(1:2);
    waypoints = [current_pos];
    visited = false(1, size(safe_points, 2));
    
    % 贪婪路径规划（最近邻）
    while sum(~visited) > 0
        unvisited_points = safe_points(:, ~visited);
        distances = vecnorm(unvisited_points - current_pos);
        
        [~, min_idx] = min(distances);
        unvisited_indices = find(~visited);
        selected_idx = unvisited_indices(min_idx);
        
        next_point = safe_points(:, selected_idx);
        
        % 检查从当前位置到下一点的路径是否安全
        if is_path_safe(current_pos, next_point, obstacles, safety_margin)
            waypoints = [waypoints, next_point];
            current_pos = next_point;
            visited(selected_idx) = true;
        else
            % 如果直接路径不安全，尝试绕行或跳过
            visited(selected_idx) = true;
        end
        
        % 限制路径点数量，避免过长
        if size(waypoints, 2) > 15
            break;
        end
    end
end

function safe = is_point_safe(point, obstacles, margin)
% 检查点是否安全（距离所有障碍物足够远）
    x = point(1);
    y = point(2);
    safe = true;
    
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        x_min = obs(1);
        y_min = obs(2);
        x_max = obs(3);
        y_max = obs(4);
        
        % 计算点到矩形的最近距离
        closest_x = max(x_min, min(x, x_max));
        closest_y = max(y_min, min(y, y_max));
        
        dist = sqrt((x - closest_x)^2 + (y - closest_y)^2);
        
        if dist < margin
            safe = false;
            return;
        end
    end
end

function safe = is_path_safe(start_point, end_point, obstacles, margin)
% 检查两点之间的直线路径是否安全
    % 在路径上采样多个点进行检查
    num_samples = 10;
    for i = 0:num_samples
        alpha = i / num_samples;
        point = (1 - alpha) * start_point + alpha * end_point;
        if ~is_point_safe(point, obstacles, margin)
            safe = false;
            return;
        end
    end
    safe = true;
end

function waypoints = generate_conservative_path(initial_state, safe_bounds)
% 生成一个保守的安全路径（在安全区域中心）
    center_x = (safe_bounds.x_min + safe_bounds.x_max) / 2;
    center_y = (safe_bounds.y_min + safe_bounds.y_max) / 2;
    
    % 小范围的安全路径
    waypoints = [
        initial_state(1:2), ...
        [center_x; center_y], ...
        [center_x + 5; center_y], ...
        [center_x + 5; center_y + 5], ...
        [center_x; center_y + 5], ...
        [center_x - 5; center_y + 5], ...
        [center_x - 5; center_y], ...
        [center_x; center_y]
    ];
end

function length = calculate_path_length(waypoints)
% 计算路径总长度
    length = 0;
    for i = 2:size(waypoints, 2)
        length = length + norm(waypoints(:, i) - waypoints(:, i-1));
    end
end

function controls = waypoints_to_controls(waypoints, dt, total_time, initial_state)
% 将路径点转换为控制序列
    steps = round(total_time / dt);
    controls = zeros(2, steps);
    
    % 控制参数
    v_max = 1.2;        % 最大线速度
    v_min = 0.4;        % 最小线速度
    omega_max = 0.5;    % 最大角速度
    
    % 为每个路径段分配时间
    total_path_length = calculate_path_length(waypoints);
    segment_lengths = zeros(1, size(waypoints, 2) - 1);
    for i = 1:size(waypoints, 2) - 1
        segment_lengths(i) = norm(waypoints(:, i+1) - waypoints(:, i));
    end
    
    % 模拟机器人状态
    current_state = initial_state;
    current_waypoint_idx = 2;
    t = 1;
    
    while t <= steps && current_waypoint_idx <= size(waypoints, 2)
        % 当前目标点
        target = waypoints(:, current_waypoint_idx);
        
        % 计算到目标的距离和角度
        dx = target(1) - current_state(1);
        dy = target(2) - current_state(2);
        dist = sqrt(dx^2 + dy^2);
        target_angle = atan2(dy, dx);
        
        % 角度差
        angle_error = wrapToPi(target_angle - current_state(3));
        
        % 如果已到达当前路径点，切换到下一个
        if dist < 1.0
            current_waypoint_idx = current_waypoint_idx + 1;
            if current_waypoint_idx > size(waypoints, 2)
                break;
            end
            continue;
        end
        
        % 计算控制量
        % 角速度控制
        omega = max(-omega_max, min(omega_max, 2.0 * angle_error));
        
        % 线速度控制（角度误差大时减速）
        if abs(angle_error) > pi/4
            v = v_min;
        else
            v = v_min + (v_max - v_min) * (1 - abs(angle_error) / (pi/4));
        end
        
        controls(:, t) = [v; omega];
        
        % 更新状态
        current_state(1) = current_state(1) + v * cos(current_state(3)) * dt;
        current_state(2) = current_state(2) + v * sin(current_state(3)) * dt;
        current_state(3) = current_state(3) + omega * dt;
        current_state(3) = wrapToPi(current_state(3));
        
        t = t + 1;
    end
    
    % 填充剩余时间 - 缓慢旋转以观察环境
    while t <= steps
        controls(:, t) = [0.3; 0.1];
        t = t + 1;
    end
end

